function [ gPN ] = gravity_krb( CNE, h, R0, f, mu, J2, J3, omegae, RSa ) %#codegen TODO:根据《应用导航算法工程基础》更新算法
%GRAVITY_KRB 按照K. R. Britting模型计算重力加速度
%
% Tips:
% # E系EUGw，初始N系ENU（可以为非地理坐标系）
%
% # Input Arguments:
% # CNE: 3*3矩阵，位置矩阵
% # h: 标量，高度，单位m
% # R0: 标量，地球长半轴，单位m
% # f: 标量，地球扁率（REF1中也称为ellipticity，记为e），无单位
% # mu: 标量，地球质量与万有引力常数之积，单位m^3/s^2
% # J2, J3: 标量，与地球质量分布相关的常量，无单位
% # omegae: 标量，地球自转角速率，单位rad/s
% # RSa: 标量，修改的到地球表面位置的经向距离，单位m
%
% Output Arguments:
% # gPN: 3*1向量，N系下的重力加速度，单位m/s^2
%
% References:
% # Paul G. Savage. Strapdown Analytics[M]. Maple Plain: Strapdown Associates, Inc., 2000: 5.1-5.4节
% #《应用导航算法工程基础》“重力模型”

% earth polar axis component of geodetic vertical unit vector
uUpYE = CNE(2, 3); % Eq. 5.3-16

% modified radial distance to earth surface location
if (nargin < 9)
    RSa = R0 / sqrt(1+(uUpYE^2)*((1-f)^2-1)); % Eq. 5.1-10
end

% radial distance to earth surface location
RS = RSa * sqrt(1+(uUpYE^2)*((1-f)^4-1)); % Eq. 5.2.1-4

% radial distance to navigation point
R = sqrt(RS^2 + 2*h*(R0^2)/RSa + h^2); % Eq. 5.2.1-5

% cosine of range vector polar coordinate angle
cosphi = uUpYE * (((1-f)^2)*RSa+h) / R; % Eq. 5.2.2-3

% modified sine of range vector polar coordinate angle
sinphimod = (RSa + h) / R; % Eq. 5.2.2-5

% cosine and modified sine of difference between geocentric and geodetic latitudes
sinschwalmod = uUpYE * (1-(1-f)^2) * RSa / R;
cosschwal = ((1-(uUpYE^2)*(1-(1-f)^2))*RSa + h) / R; % Eq. 5.2.3-5

% gravity components in polar coordinates
if h >= 0
    [gr, gphidsinphi] = gravity_polarcoord(R, cosphi, R0, mu, J2, J3);
else
    [grs, gphidsinphis] = gravity_polarcoord(RS, cosphi, R0, mu, J2, J3);
    gr = R/RS*grs;
    gphidsinphi = R/RS*gphidsinphis; % Eq. 5.4-2
end

% north and vertical gravity components
gUp = gr*cosschwal - gphidsinphi*sinphimod*sinschwalmod*(1-uUpYE^2);
gNorthmod = -gphidsinphi*sinphimod*cosschwal - gr*sinschwalmod; % Eq. 5.4-4

% north and vertical plumb-bob gravity components
gPNorthmod = gNorthmod - (RSa+h)*(omegae^2)*uUpYE;
gPUp = gUp + (RSa+h)*(omegae^2)*(1-uUpYE^2); % Eq. 5.4.1-9

% N frame plumb-bob gravity components
gPN = [gPNorthmod*CNE(2, 1), gPNorthmod*CNE(2, 2), gPUp]'; % Eq. 5.4.1-11
end

function [gr, gphidsinphi] = gravity_polarcoord(R, cosphi, R0, mu, J2, J3)
gr = -(mu/(R^2)) * (1 - 3/2*J2*((R0/R)^2)*(3*(cosphi^2)-1) - 2*J3*((R0/R)^3)*cosphi*(5*(cosphi^2)-3));
gphidsinphi = 3 * (mu/(R^2)) * ((R0/R)^2) * (J2*cosphi + 1/2*J3*R0/R*(5*(cosphi^2)-1)); % Eq. 5.4-1
end   